#include <math.h>
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <netdb.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/ioctl.h>
#include <fcntl.h>

#include "NeighborDiscover.h"
#include "ManageRoute.h"
#include "Constant.h"
#include "Beacon.h"

#define MAX(a,b) a>b ? a:b

int linkNeighbors[256]; //store pre neighbor's link quality(rssi)
int currentNeighbors[256]; //store current link quality(rssi) of neighbors

char linkQualityMap[21]={100,85,75,65,50,40,35,30,25,20,17,15,14,13,13,12,12,11,11,10,10};

int neighborTable[256][256]; // store all link quality of link in the network
int tmpTable[256][256]; //store a symmetrical copy of neighborTable when find router

int tableAge[256];

char backUpFileName[50] = "/root/staticRoute.txt";

pthread_mutex_t tableMutex;

void setMyNeighbor(int* linkNeighbors);

void int2str(int a, char* b)
{
	memcpy(b, &a, 4);
}

int str2int(char* b)
{
	int a;
	memcpy(&a,b,4);
	return a;
}

void readMyAge()
{
	FILE* fp = fopen(backUpFileName, "r");
	if(fp == NULL)
	{
		tableAge[HOSTID] = 0;
		fp = fopen(backUpFileName, "w");
		fprintf(fp, "%d", 0);
		fclose(fp);
	}
	else
	{
		fscanf(fp,"%d", &tableAge[HOSTID]);
		fclose(fp);
	}
	printf("readMyAge: My age is %d\n", tableAge[HOSTID]);
	
}

void writeMyAge()
{
	printf("wirteMyAge: My age is %d\n", tableAge[HOSTID]);
	FILE* fp = fopen(backUpFileName, "w");
	fprintf(fp, "%d", tableAge[HOSTID]);
	fclose(fp);
}

void neighborDiscoverInit()
{
	pthread_mutex_init(&tableMutex, NULL);
	readMyAge();
	int i,j;
	for(i=1;i<256;i++)
		for(j=1;j<256;j++)
			neighborTable[i][j]=1000000;
}

void* neighborDiscover(void *null)
{
	int i;
	char flag;
	int file_desc;

	char buff[512];
	int len;

	file_desc = open("/dev/char_dev", 0);
	if(file_desc < 0)
	{
		printf("Can't open device file: /dev/char_dev\n");
		exit(-1);
	}

	while(1)
	{
		//read currentNeighbors from the rt73 driver
		bzero(currentNeighbors, sizeof(currentNeighbors));
		len = read(file_desc, buff, sizeof(buff));

		printf("current neighbors:");
		for (i=0; i<len; i+=2)
		{
			printf("(%d,%d,%d) ",0x00ff&buff[i], linkNeighbors[0x00ff&buff[i]], 0x00ff&buff[i+1]);
			currentNeighbors[0x00ff&buff[i]] = 0x00ff&buff[i+1];
		}
		printf("\n");

		flag = 0;
		for (i=1; i<256; i++)
			if (!(currentNeighbors[i]>=195 && linkNeighbors[i]>=195) && abs(currentNeighbors[i]-linkNeighbors[i])>=5)
				flag = 1;
		
		if(flag)
		{
			memcpy(linkNeighbors,currentNeighbors, sizeof(linkNeighbors));
			setMyNeighbor(linkNeighbors);
		}

		sleep(5);
	}
	return (void *)NULL;
}

void findRoute(){
	printf("--------------start of findRoute--------------\n");
	int i;
	int j;
	int k;
	char father[256];
	int L[256];

	int LTable[256][256];

	char used[256];


	for (i=1; i<256; i++){
      for (j=i; j<256; j++){
	  	  LTable[i][j] = LTable[j][i]=1000000;
          if (neighborTable[i][j]<1000000){
             printf("%d->%d:%d    ",i,j,neighborTable[i][j]);
          }
		  if (neighborTable[j][i]<1000000){
             printf("%d->%d:%d",j,i,neighborTable[j][i]);
          }
		  if (neighborTable[i][j]<1000000 || neighborTable[j][i]<1000000){
		  	 printf("\n");
		  }
		  tmpTable[i][j] = tmpTable[j][i] = MAX(neighborTable[i][j],neighborTable[j][i]);
      }
 	}

	int minV,minI;
	
	//calculate the length of route from each my neighbor to others
	for (k=1; k<256; k++){
		if (neighborTable[HOSTID][k]<1000000){
			memset(used,0,sizeof(used));
			for(i=1;i<256;i++){
				father[i]=0;
				L[i]=1000000;
			}
			
			father[k]=k;
			L[k]=tmpTable[HOSTID][k];

	
			minV = 1;
			while( minV<1000000 ){
				minV = 1000000;
				for(i=1; i<256; i++){
					if(used[i]==0 && L[i]<minV){
						minV=L[i];
						minI=i;
					}
				}

				if(minV<1000000){
				  used[minI] = 1;
					for(i=1; i<256; i++){
						j = tmpTable[minI][i];
						if( j+minV < L[i] ){
							L[i] = j + minV;
							father[i] = father[minI];
						}
					}
				}
			}
			for(i=1;i<256;i++){
				LTable[k][i]=L[i];
			}
		}
	}

    //calculate the length of route from my to others
	memset(used,0,sizeof(used));
	for(i=1;i<256;i++){
		father[i]=0;
		L[i]=1000000;
	}
	for (k=1; k<256; k++){
		if (neighborTable[HOSTID][k]<1000000){
			father[k]=k;
			L[k]=tmpTable[HOSTID][k];
		}
	}
	minV = 1;
	while( minV<1000000 ){
		minV = 1000000;
		for(i=1; i<256; i++){
			if(used[i]==0 && L[i]<minV){
				minV=L[i];
				minI=i;
			}
		}

		if(minV<1000000){
		  used[minI] = 1;
			for(i=1; i<256; i++){
				j = tmpTable[minI][i];
				if( j+minV < L[i] ){
					L[i] = j + minV;
					father[i] = father[minI];
				}
			}
		}
	}
	
	
	for( i=1; i<256; i++){
		if(L[i]<1000000 && i!=HOSTID){
			j = get_gw(i);
			printf("%d---->%d father=%d L=%d\n",HOSTID,i,father[i],L[i]);
			if( (j==0) || ((j!=father[i]) && (LTable[j][i]+neighborTable[HOSTID][j])>(L[i]*1.4)) ){
				add_router(i, father[i]);
				//printf("after add_router\n");
			}
		}
	}
	printf("---------------End of findRoute---------------\n");
}


void setMyNeighbor(int* linkNeighbors){
	printf("\n--------------Start of setMyNeighbor--------------\n");
	int i,tmp;
	pthread_mutex_lock( &tableMutex );
	for (i=1; i<256; i++){
		tmp = linkNeighbors[i];
		if (tmp==0){
			neighborTable[HOSTID][i] = 1000000;
		}
		else{
			if (tmp<180)
				tmp = 180;
			else if(tmp>200){
				tmp=200;
			}

			neighborTable[HOSTID][i] = linkQualityMap[tmp-180];
		}
	}

	tableAge[HOSTID]++;
    writeMyAge();

	findRoute();

	set_beacon_counter(2);
	pthread_mutex_unlock( &tableMutex );
	
	printf("---------------End of setMyNeighbor---------------\n\n");
}



void updateTable(char* buff,int len){
	printf("\n--------------Start of updateTable--------------\n");

	int p,id,age,i;
	p=0;
	char flag;

	char unneighbor_flag;

	pthread_mutex_lock( &tableMutex );
	flag = 0;
	unneighbor_flag=0;
	while(p<len){
		id = 0x00ff & buff[p++];
		age = str2int(&buff[p]);
		p=p+4;

		if (age>tableAge[id]){
			flag = 1;
			for(i=1; i<256; i++){
				neighborTable[id][i]=1000000;
			}
		    while( (buff[p])!=0 ){
				neighborTable[id][ 0x00ff&buff[p] ]=0x00ff&buff[p+1];
				p+=2;
		    }
			tableAge[id]=age;
			printf("%d's age becomes to %d\n",id,age);
			
			if(neighborTable[id][HOSTID]<1000000&&neighborTable[HOSTID][id]<1000000){
				unneighbor_flag = 1;
			}
		}
		else{
			while( buff[p]!=0 ){
				p++;
			}
		}
		p++;
	}


	if (flag){
		findRoute();
	}
	if( unneighbor_flag ){
		set_beacon_counter(4);
	}
	pthread_mutex_unlock( &tableMutex );
	printf("---------------End of updateTable---------------\n\n");
}

int getTableInfo(char* buff){
	int len,i,j;

	pthread_mutex_lock( &tableMutex );
	len = 0;
	for (i=1; i<256; i++){
		if (tableAge[i]>0){
			buff[len++]=i;
			int2str(tableAge[i],buff+len);
			len = len + 4;
			for (j=1; j<256; j++){
				if ( neighborTable[i][j]<1000000 ){
					buff[len++] = j;
					buff[len++] = 0x00ff&neighborTable[i][j];
				}
			}
			buff[len++]=0;
		}
	}
	pthread_mutex_unlock( &tableMutex );

	return len;
}
